package com.tjvib.util;

import android.util.Log;

import java.nio.ByteBuffer;
import java.nio.ByteOrder;

public class ZLBUSDataDecoder {
    // ZLBUS输出类型
    private static final int IMU_DATA = 0x01;
    private static final int ICSTATUS_DATA = 0x02;
    private static final int BATTERY_DATA = 0x03;
    private static final int ANT_DATA = 0x04;
    private static final int CMD_DATA = 0x05;
    // IMU数据输出格式设置
    private static boolean IMU_DATA_FORMAT_SET = false;
    private static boolean TIMESTAMP_OUT_EN = false;
    private static boolean QUATERNION_OUT_EN = false;
    private static boolean RPY_OUT_EN = false;
    private static boolean ACC_OUT_EN = false;
    private static boolean GYRO_OUT_EN = false;
    private static boolean MAG_OUT_EN = false;
    private static boolean LIN_OUT_EN = false;
    private static boolean TEMP_OUT_EN = false;


    public static boolean get_imu_data_format_set() {
        return IMU_DATA_FORMAT_SET;
    }

    public static int decodeDataTypeID(byte[] data) {
        if (data[0] != (byte) 0xAA) {
            return -1; // 帧头错误
        }

        if (data[1] == (byte) 0x10) {
            return IMU_DATA;
        } else if (data[1] == (byte) 0x11) {
            return ICSTATUS_DATA;
        } else if (data[1] == (byte) 0x14) {
            return BATTERY_DATA;
        } else if (data[1] == (byte) 0x15) {
            return ANT_DATA;
        } else if (data[1] == (byte) 0xD5) {
            return CMD_DATA;
        }
        return -1;
    }


    public static String decodeRecvIMUDataStr(byte[] data) {
        int package_index=0;
        // 帧头 0xAA
        if (data[0] != (byte) 0xAA) {
            return null; // 帧头错误
        }
        // 解码IMU数据上报
        if (data[1] == (byte) 0x10) {
            // 移除帧头和校验码
            byte[] payload = new byte[data.length - 8 - 1]; // 减去帧头 指令 数据长度 子指令 MT_ID 流水号 校验码长度
            System.arraycopy(data, 8, payload, 0, payload.length); // 从索引7开始复制，跳过帧头 指令 数据长度 子指令 MT_ID 流水号 校验码

            StringBuilder sb = new StringBuilder();

            // 时间戳
            if (TIMESTAMP_OUT_EN) {
                float timestamp = ByteBuffer.wrap(payload, package_index, 4).order(ByteOrder.LITTLE_ENDIAN).getFloat();
                sb.append("Timestamp: ").append(timestamp).append(" ms, ");
                sb.append("\n");
                package_index +=  4;
            }
            // 四元数
            if (QUATERNION_OUT_EN) {
                float[] quaternion = new float[4];
                for (int i = 0; i < 4; i++) {
                    quaternion[i] = ByteBuffer.wrap(payload, 4 * i + package_index, 4).order(ByteOrder.LITTLE_ENDIAN).getFloat();
                    if (i == 0) {
                        sb.append("Quaternion ").append(": ").append(quaternion[i]).append(", ");
                    } else {
                        sb.append(quaternion[i]).append(", ");
                    }
                }
                sb.append("\n");
                package_index += 16;
            }

            // 欧拉角
            if (RPY_OUT_EN) {
                float[] eulerAngle = new float[3];
                for (int i = 0; i < 3; i++) {
                    eulerAngle[i] = ByteBuffer.wrap(payload, package_index + 4 * i, 4).order(ByteOrder.LITTLE_ENDIAN).getFloat();
                    if (i == 0) {
                        sb.append("Euler Angle ").append(": ").append(eulerAngle[i]).append(", ");
                    } else {
                        sb.append(eulerAngle[i]).append(", ");
                    }
                }
                sb.append("\n");
                package_index += 12;
            }


            // 加速度
            if (ACC_OUT_EN) {
                float[] acceleration = new float[3];
                for (int i = 0; i < 3; i++) {
                    acceleration[i] = ByteBuffer.wrap(payload, package_index + 4 * i, 4).order(ByteOrder.LITTLE_ENDIAN).getFloat();
                    if (i == 0) {
                        sb.append("Acceleration ").append(": ").append(acceleration[i]).append(" g, ");
                    } else {
                        sb.append(acceleration[i]).append(" g, ");
                    }
                }
                sb.append("\n");
                package_index += 12;
            }

            // 陀螺仪
            if (GYRO_OUT_EN) {
                float[] gyroscope = new float[3];
                for (int i = 0; i < 3; i++) {
                    LogUtil.e("acceleration"+gyroscope.length+" package_index="+package_index);
                    gyroscope[i] = ByteBuffer.wrap(payload, package_index + 4 * i, 4).order(ByteOrder.LITTLE_ENDIAN).getFloat();
                    if (i == 0) {
                        sb.append("Gyroscope ").append(": ").append(gyroscope[i]).append(", ");
                    } else {
                        sb.append(gyroscope[i]).append(", ");
                    }
                }
                sb.append("\n");
                package_index += 12;
            }

            // 磁力计
            if (MAG_OUT_EN) {
                float[] magnetic = new float[3];
                for (int i = 0; i < 3; i++) {
                    magnetic[i] = ByteBuffer.wrap(payload, package_index + 4 * i, 4).order(ByteOrder.LITTLE_ENDIAN).getFloat();
                    if (i == 0) {
                        sb.append("Magnetic ").append(": ").append(magnetic[i]).append(" μT, ");
                    } else {
                        sb.append(magnetic[i]).append(" μT, ");
                    }
                }
                sb.append("\n");
                package_index += 12;
            }

            // 线性加速度
            if (LIN_OUT_EN) {
                float[] linearAcceleration = new float[3];
                for (int i = 0; i < 3; i++) {
                    linearAcceleration[i] = ByteBuffer.wrap(payload, package_index + 4 * i, 4).order(ByteOrder.LITTLE_ENDIAN).getFloat();
                    if (i == 0) {
                        sb.append("Linear Accel ").append(": ").append(linearAcceleration[i]).append(", ");
                    } else {
                        sb.append(linearAcceleration[i]).append(", ");
                    }
                }
                sb.append("\n");
                package_index += 12;
            }

            // 温度
            if (TEMP_OUT_EN) {
                float temperature = ByteBuffer.wrap(payload, package_index, 4).order(ByteOrder.LITTLE_ENDIAN).getFloat();
                sb.append("Temperature: ").append(temperature).append(" °C").append("\n");
            }

            return sb.toString();
        }

        //解码IC状态
        else if (data[1] == (byte) 0x11) {
            StringBuilder sb = new StringBuilder();
            sb.append("Received IC Status Message");
            return sb.toString();
        }
        //解码电池上报
        else if (data[1] == (byte) 0x14) {
            StringBuilder sb = new StringBuilder();
            String sb_battery = Integer.toString(data[8] &0xFF);
            sb.append("电量：").append(sb_battery);
            return sb.toString();
        }
        //解码ZLBUS ADC数据包
        else if (data[1] == (byte) 0x15) {
            StringBuilder sb = new StringBuilder();
            sb.append("Received ANT Response");
            return sb.toString();
        }
        // 解码ZLBUS指令
        else if (data[1] == (byte) 0xD5) {
            StringBuilder sb = new StringBuilder();
            if (data[4] == (byte) 0x00) {
                sb.append("Received IMU DATA FORMAT Set CMD");
             } else  {
                sb.append("Received CMD");
            }
            return sb.toString();
        }
        else {
            return null;
        }
    }

    public static String decodeRecvIMUDataRaw(byte[] data) {
        int package_index=0;
        // 帧头 0xAA
        if (data[0] != (byte) 0xAA) {
            return null; // 帧头错误
        }
        // 解码IMU数据上报
        if (data[1] == (byte) 0x10) {
            // 移除帧头和校验码
            byte[] payload = new byte[data.length - 8 - 1]; // 减去帧头 指令 数据长度 子指令 MT_ID 流水号 校验码长度
            System.arraycopy(data, 8, payload, 0, payload.length); // 从索引7开始复制，跳过帧头 指令 数据长度 子指令 MT_ID 流水号 校验码

            StringBuilder sb = new StringBuilder();

            // 时间戳
            if (TIMESTAMP_OUT_EN) {
                float timestamp = ByteBuffer.wrap(payload, package_index, 4).order(ByteOrder.LITTLE_ENDIAN).getFloat();
                sb.append(timestamp).append(",");
                package_index +=  4;
            }
            // 四元数
            if (QUATERNION_OUT_EN) {
                float[] quaternion = new float[4];
                for (int i = 0; i < 4; i++) {
                    quaternion[i] = ByteBuffer.wrap(payload, 4 * i + package_index, 4).order(ByteOrder.LITTLE_ENDIAN).getFloat();
                    sb.append(quaternion[i]).append(", ");
                }
                package_index += 16;
            }

            // 欧拉角
            if (RPY_OUT_EN) {
                float[] eulerAngle = new float[3];
                for (int i = 0; i < 3; i++) {
                    eulerAngle[i] = ByteBuffer.wrap(payload, package_index + 4 * i, 4).order(ByteOrder.LITTLE_ENDIAN).getFloat();
                    sb.append(eulerAngle[i]).append(",");
                }
                package_index += 12;
            }


            // 加速度
            if (ACC_OUT_EN) {
                float[] acceleration = new float[3];
                for (int i = 0; i < 3; i++) {
                    acceleration[i] = ByteBuffer.wrap(payload, package_index + 4 * i, 4).order(ByteOrder.LITTLE_ENDIAN).getFloat();
                    sb.append(acceleration[i]).append(",");
                }
                package_index += 12;
            }

            // 陀螺仪
            if (GYRO_OUT_EN) {
                float[] gyroscope = new float[3];
                for (int i = 0; i < 3; i++) {
                    gyroscope[i] = ByteBuffer.wrap(payload, package_index + 4 * i, 4).order(ByteOrder.LITTLE_ENDIAN).getFloat();
                    sb.append(gyroscope[i]).append(",");
                }
                package_index += 12;
            }

            // 磁力计
            if (MAG_OUT_EN) {
                float[] magnetic = new float[3];
                for (int i = 0; i < 3; i++) {
                    magnetic[i] = ByteBuffer.wrap(payload, package_index + 4 * i, 4).order(ByteOrder.LITTLE_ENDIAN).getFloat();
                    sb.append(magnetic[i]).append(",");
                }
                package_index += 12;
            }

            // 线性加速度
            if (LIN_OUT_EN) {
                float[] linearAcceleration = new float[3];
                for (int i = 0; i < 3; i++) {
                    linearAcceleration[i] = ByteBuffer.wrap(payload, package_index + 4 * i, 4).order(ByteOrder.LITTLE_ENDIAN).getFloat();
                    sb.append(linearAcceleration[i]).append(", ");
                }
                package_index += 12;
            }

            // 温度
            if (TEMP_OUT_EN) {
                float temperature = ByteBuffer.wrap(payload, package_index, 4).order(ByteOrder.LITTLE_ENDIAN).getFloat();
                sb.append(temperature).append("\n");
            }
            return sb.toString();
        } else {
            return null;
        }
    }


    public static String decodeSendData(byte[] data) {
        StringBuilder sb = new StringBuilder();
        // 帧头 0xAA
        if (data[0] != (byte) 0xAA) {
            return null; // 帧头错误
        }
        // 解码IMU数据上报格式
        if (data[1] == (byte) 0xD5) {
            if (data[4] == (byte) 0x00) {
                IMU_DATA_FORMAT_SET = true;
                TIMESTAMP_OUT_EN = ((data[10] & 0x80) >> 7) == 1;
                TEMP_OUT_EN = ((data[8] & 0x40) >> 6) == 1;
                LIN_OUT_EN = ((data[7] & 0x20) >> 5) == 1;
                MAG_OUT_EN = ((data[7] & 0x10) >> 4) == 1;
                GYRO_OUT_EN = ((data[7] & 0x08) >> 3) == 1;
                ACC_OUT_EN = ((data[7] & 0x04) >> 2) == 1;
                RPY_OUT_EN = ((data[7] & 0x02) >> 1) == 1;
                QUATERNION_OUT_EN = (data[7] & 0x01) == 1;
                Log.w("TAG","收到IMU配置:" + TIMESTAMP_OUT_EN + TEMP_OUT_EN + LIN_OUT_EN + MAG_OUT_EN + GYRO_OUT_EN + ACC_OUT_EN + RPY_OUT_EN + QUATERNION_OUT_EN);
                sb.append("IMU DATA FORMAT set");
                return sb.toString();
            }
        }
        else {
            sb.append("Received Send CMD");
            return sb.toString();
        }
       return null;
    }
}
